#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <cstring>
#include <cstdlib>
#include <pcl/pcl_macros.h>

#include <iostream>
#include <fstream>  // 包含文件流头文件
#include <chrono>


// velodyne的点云格式
struct VelodynePointXYZIRT {
    PCL_ADD_POINT4D

    PCL_ADD_INTENSITY;
    uint16_t ring;
    float time;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (uint16_t, ring, ring)
    (float, time, time)
)


struct PointXYZIRT {
    PCL_ADD_POINT4D;                  // XYZ + intensity

    PCL_ADD_INTENSITY;
    uint16_t ring;                    // ring number
    float time;                 // time stamp
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (uint16_t, ring, ring)
    (float, time, time)
)

pcl::PointCloud<pcl::PointXYZI> input_cloud; // 假设这是你的输入点云
pcl::PointCloud<PointXYZIRT> output_cloud;

static int count_log = 0;

int num_channels = 32; // 例如，32线激光雷达
float freq = 40.0f;       // 例如，转速为10freq
float upper_fov = 10.0f;
float lower_fov = -30.0f;


rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_carla_xyzi;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_conver_xyzirt;

int getRingNumber(float azimuth_xy_z, int num_channels, float upper_fov, float lower_fov) {
    // 俯仰角范围,0.5为仰角计算误差
    if (azimuth_xy_z < (lower_fov-0.5) || azimuth_xy_z > (upper_fov+0.5)) {
        return -1;
    }

    // carla的垂直射线的分布是等差数列，计算公差
    float dt = (upper_fov - lower_fov) / (num_channels - 1);

    // 通道号我直接按从上到下的顺序排列，0～num_channels-1
    float temp_ring1 = ((-(azimuth_xy_z - upper_fov)) / dt);
    int temp_ring = static_cast<int>(std::round(temp_ring1));

    // 为上一步计算可能出错的情况做容错，保证返回的ring在0～num_channels-1之间
    temp_ring = std::min(std::max(temp_ring, 0), num_channels - 1);
    return temp_ring;
}

float gettime(float azimuth_x_y, float freq) {
    // 计算一圈的周期
    float time_per_rotation = 1 / freq;

    // 计算一个角度花费 的时间
    float time_per_degree = time_per_rotation / 360.0;

    // 计算角度花费的时间
    float time = azimuth_x_y * time_per_degree;

    return time;
}

template<typename T>
bool has_nan(T point) {
    if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) {
        return true;
    } else {
        return false;
    }
}

void publish_points(const pcl::PointCloud<PointXYZIRT>::Ptr &new_pc, const sensor_msgs::msg::PointCloud2 &old_msg) {
    // pc properties
    new_pc->is_dense = true;

    // publish
    sensor_msgs::msg::PointCloud2 pc_new_msg;
    pcl::toROSMsg(*new_pc, pc_new_msg);
    pc_new_msg.header = old_msg.header;
    pc_new_msg.header.frame_id = "velodyne";

    pub_conver_xyzirt->publish(pc_new_msg);
}

void Handler_XYZI(sensor_msgs::msg::PointCloud2 pc_msg) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr pc(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<PointXYZIRT>::Ptr pc_new(new pcl::PointCloud<PointXYZIRT>());
    pcl::fromROSMsg(pc_msg, *pc);

    // to new pointcloud

    if (count_log % 20 == 0) std::cout << "one frame points.size():" <<pc->points.size()<< std::endl;
    count_log ++;
    if (count_log >= 100000) count_log = 0;
    for (int point_id = 0; point_id < pc->points.size(); ++point_id) {
        if (has_nan(pc->points[point_id])) {
            std::cout << "has nan" << std::endl;
            continue;
        }
        PointXYZIRT new_point;
        new_point.x = pc->points[point_id].x;
        new_point.y = pc->points[point_id].y;
        new_point.z = pc->points[point_id].z;
        new_point.intensity = pc->points[point_id].intensity;

        // Calculate the x_y azimuth
        float azimuth_x_y = atan2(new_point.y, new_point.x) * 180.0 / M_PI;
        if (azimuth_x_y < 0) {
            azimuth_x_y += 360.0f;
        }

        // 计算ring的时间偏差
        new_point.time = gettime(azimuth_x_y, freq);

        // Calculate the xy_z azimuth
        float azimuth_xy_z = atan(new_point.z / sqrt(new_point.x * new_point.x + new_point.y * new_point.y)) * 180 / M_PI;

        // Get the ring number
        new_point.ring = getRingNumber(azimuth_xy_z, num_channels, upper_fov, lower_fov);

        if(new_point.ring == -1) continue;

        // std::cout << "azimuth_x_y:" << azimuth_x_y << " azimuth_xy_z:" << azimuth_xy_z << " ring:" << new_point.ring <<" stamp time:" << pc_msg.header.stamp.sec << " time:" << new_point.time << std::endl;

        pc_new->points.push_back(new_point);
    }
    // std::cout << "one frame end" << std::endl;
    publish_points(pc_new, pc_msg);
}

int main(int argc, char **argv) {

    // // 创建并打开文件流
    // std::ofstream file("output.txt");
    // if (!file.is_open()) {
    //     std::cerr << "Failed to open file for writing!" << std::endl;
    //     return 1;
    // }

    // // 保存原始的cout缓冲区
    // std::streambuf* original_cout_buffer = std::cout.rdbuf();

    // // 将cout重定向到文件
    // std::cout.rdbuf(file.rdbuf());

    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("converter_to_xyzirt");
    sub_carla_xyzi = node->create_subscription<sensor_msgs::msg::PointCloud2>("/carla_pointcloud", 1, Handler_XYZI);
    
    //pubRobosensePC = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 1);
    pub_conver_xyzirt = node->create_publisher<sensor_msgs::msg::PointCloud2>("/carla/velodyne_points", 1);
    RCLCPP_INFO(node->get_logger(), "Listening to /carla_xyzi_points ......");
    rclcpp::spin(node);
    return 0;
}
